#include "app_distance_pid.h"
#include "pid.h"
#include "motor/hw_motor.h"
#include "motor/hw_encoder.h"



PID distance_pid;

void distance_pid_init(void) {
    pid_init(&distance_pid, 75, 2, 10, 9999, 9999, DEFAULT_ANGLE);
}

PID* get_distance_pid(void) {
    return &distance_pid;
}

/************************************************
功能：PID电机定距控制器
参数：target_speed = 目标值
返回：对应PID的地址
************************************************/
PID motor_distance_control(int target)
{
	int PWM=0;
	int target_pulses = 0;
	char buff[50]={0};

	//将角度转换为脉冲
	target_pulses = target / DEGREES_PER_PULSE;

    PWM = pid_calc(&distance_pid, target_pulses, get_encoder_temp_count());
    
    if( PWM > 0 )
    {
        set_motor(PWM, 0);
    }
    else if( PWM < 0 )
    {
        PWM = -PWM;
        set_motor(0, PWM);
    }
	
	return distance_pid;
}